#include "ros/ros.h"
#include <iostream>
#include <fstream>
#include <algorithm>
#include <math.h>

#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <nav_msgs/Path.h>
#include <tf/transform_datatypes.h>

std::string global_plan_target = "global_plan.txt";
std::string local_plan_target = "local_plan.txt";
std::string odom_combined_target = "odom_combined.txt";
std::string amcl_target = "amcl.txt";

std::ofstream global_plan_file, local_plan_file, odom_combined_file, amcl_file;

int global_plan_id = 0,  local_plan_id = 0, odom_combined_point_id = 0, amcl_point_id = 0;
// 工具函数

bool openFile(std::ofstream& file, const std::string& target_file){
    file.open(target_file);
    if(!file.is_open()){
        std::cerr << "Error opening temporary file for writing." << std::endl;
        return false;
    }
    return true;
}

void global_plan_callback(const nav_msgs::Path& data){
    std::string insert_line;
    if(global_plan_id != 0){
        insert_line.append("[restart global plan]");
        global_plan_file << insert_line << std::endl;
        insert_line.erase(insert_line.begin(), insert_line.end());
    }
    int len = data.poses.size(); 
    for(int i = 0; i < len; i++){
        insert_line.append("{ " + std::to_string(global_plan_id) + ", " + std::to_string(i) + ", " + 
                            std::to_string(data.header.stamp.toSec()) + ", {" + std::to_string(data.poses[i].pose.position.x) +
                            ", " + std::to_string(data.poses[i].pose.position.y) + ", " + 
                            std::to_string(tf::getYaw(data.poses[i].pose.orientation)) + "} }");
        global_plan_file << insert_line << std::endl;
        insert_line.erase(insert_line.begin(), insert_line.end());
    }
    global_plan_id++;
}
void local_plan_callback(const nav_msgs::Path& data){
    std::string insert_line;
    if(local_plan_id != 0){
        insert_line.append("[restart local plan]");
        local_plan_file << insert_line << std::endl;
        insert_line.erase(insert_line.begin(), insert_line.end());
    }
    int len = data.poses.size(); 
    for(int i = 0; i < len; i++){
        insert_line.append("{ " + std::to_string(global_plan_id) + ", " + std::to_string(i) + ", " + 
                            std::to_string(data.header.stamp.toSec()) + ", {" + std::to_string(data.poses[i].pose.position.x) +
                            ", " + std::to_string(data.poses[i].pose.position.y) + ", " + 
                            std::to_string(tf::getYaw(data.poses[i].pose.orientation)) + "} }");
        local_plan_file << insert_line << std::endl;
        insert_line.erase(insert_line.begin(), insert_line.end());
    }
    local_plan_id++;
}
void odom_combined_callback(const geometry_msgs::PoseWithCovarianceStamped& data){
    std::string insert_line;
    insert_line.append("{ " + std::to_string(odom_combined_point_id) + ", " + std::to_string(data.header.stamp.toSec()) + 
                        ", {" + std::to_string(data.pose.pose.position.x) + ", " + std::to_string(data.pose.pose.position.x) + 
                        ", " + std::to_string(tf::getYaw(data.pose.pose.orientation)) + "} }");
    odom_combined_file << insert_line << std::endl;
    insert_line.erase(insert_line.begin(), insert_line.end());
    odom_combined_point_id++;
}
void amcl_callback(const geometry_msgs::PoseWithCovarianceStamped& data){
    std::string insert_line;
    insert_line.append("{ " + std::to_string(amcl_point_id) + ", " + std::to_string(data.header.stamp.toSec()) + 
                        ", {" + std::to_string(data.pose.pose.position.x) + ", " + std::to_string(data.pose.pose.position.x) + 
                        ", " + std::to_string(tf::getYaw(data.pose.pose.orientation)) + "} }");
    amcl_file << insert_line << std::endl;
    insert_line.erase(insert_line.begin(), insert_line.end());
    amcl_point_id++;
}
int main(int argc, char** argv){
    ros::init(argc, argv, "record_realtime_path");
    ros::NodeHandle nh;
    if(openFile(global_plan_file, global_plan_target)){
        ros::Subscriber global_plan_sub = nh.subscribe("/move_base/TebLocalPlannerROS/global_plan", 1, global_plan_callback);
        global_plan_file << "[global plan point struct: { plan_id, point_id, time, {x, y, yaw} }]" << std::endl;
    }
    else{
        ROS_ERROR("Error: can't open global_plan_target %s\n",global_plan_target.c_str());
    }
    if(openFile(local_plan_file, local_plan_target)){
        ros::Subscriber global_plan_sub = nh.subscribe("/move_base/TebLocalPlannerROS/local_plan", 1, local_plan_callback);
        local_plan_file << "[local plan point struct: { plan_id, point_id, time, {x, y, yaw} }]" << std::endl;   
    }
    else{
        ROS_ERROR("Error: can't open local_plan_target %s\n",local_plan_target.c_str());
    }
    if(openFile(odom_combined_file, odom_combined_target)){
        ros::Subscriber global_plan_sub = nh.subscribe("/robot_pose_ekf/odom_combined", 1, odom_combined_callback);
        odom_combined_file << "[odom_combined point struct: { point_id, time, {x, y, yaw} }]" << std::endl;
    }
    else{
        ROS_ERROR("Error: can't open odom_combined_target %s\n",odom_combined_target.c_str());
    }
    if(openFile(amcl_file, amcl_target)){
        ros::Subscriber global_plan_sub = nh.subscribe("/amcl", 1, amcl_callback);
        amcl_file << "[amcl point struct: { point_id, time, {x, y, yaw} }]" << std::endl;
    }
    else{
        ROS_ERROR("Error: can't open amcl_target %s\n",amcl_target.c_str());
    }
    ros::spin();
}